#ifndef DEPTH_CALCULATING_HPP
#define DEPTH_CALCULATING_HPP

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <math.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>

using namespace std;
using namespace cv;
using namespace pcl;

class DepthCal
{
public:
    DepthCal();
    ~DepthCal();
    bool stereo_correction(const Mat& input_img_l, const Mat& input_img_r);
    bool get_roi_img(const Mat& input_img_l, const Mat& input_img_r, const Rect& rect_l, Rect& rect_r, 
                        Rect& rect_roi, Mat& roi_img_l, Mat& roi_img_r);
    bool disparity2pointcloud(const Mat& disparity_matrix, const Rect& rect_roi, pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud);
    bool stereo_match(const Mat& input_img_l, const Mat& input_img_r);
    bool stereo_match(const Mat& input_img_l, const Mat& input_img_r, const Rect& rect_l);
    void test(const Mat& src);
    inline Mat get_disparity()const{return disparity_matrix;}
    inline Mat get_rectifyImageL()const{return rectifyImageL;}
    inline Mat get_rectifyImageR()const{return rectifyImageR;}
    inline pcl::PointCloud<pcl::PointXYZ>::Ptr get_point_cloud()const{return point_cloud;}
private:
    Mat cameraMatrixL = (Mat_<double>(3, 3) << 
            1.27267825e+03, 0.00000000e+00, 9.88387071e+02, 
            0.00000000e+00, 1.27265778e+03, 6.36566555e+02, 
            0.00000000e+00, 0.00000000e+00, 1.00000000e+00);
    Mat distCoeffL = (Mat_<double>(1, 5) << 
            -1.29939435e-01, 1.51095405e-01, -2.01009309e-04, 2.48735316e-05, -3.90017117e-02);
    Mat cameraMatrixR = (Mat_<double>(3, 3) << 
            1.27386637e+03, 0.00000000e+00, 9.65632284e+02, 
            0.00000000e+00, 1.27367885e+03, 6.39194967e+02, 
            0.00000000e+00, 0.00000000e+00, 1.00000000e+00);
    Mat distCoeffR = (Mat_<double>(1, 5) << 
            -0.13019095, 0.15059209, 0.00074157, -0.00023195, -0.03407628);
    Mat T = (Mat_<double>(3, 1) << 
            -300.19742601, 
            -2.21131235, 
            -1.19155963);
    Mat R = (Mat_<double>(3, 3) << 
            9.99904579e-01, -1.33709919e-02, -3.47135126e-03,
            1.33741257e-02, 9.99910174e-01, 8.81120659e-04,
            3.45925798e-03, -9.27462870e-04, 9.99993587e-01);
    
    const double ball_diameter = 230; // 球的直径，单位mm
    const int imageWidth = 1920; // 原始图像的宽
    const int imageHeight = 1200; // 原始图像的高
    const int blockSize = 9, mindisparity = 60, ndisparities = 352, img_channels = 3; // SGBM中的参数
    Size imageSize = Size(imageWidth, imageHeight);
    Mat Rl, Rr, Pl, Pr, Q;                  // 校正旋转矩阵R，投影矩阵P 重投影矩阵Q
    Rect validROIL, validROIR;              // 图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
    Mat mapLx, mapLy, mapRx, mapRy;         // 映射表
    Mat rectifyImageL, rectifyImageR;       // 矫正后的图像
    Mat disparity_matrix;                   // 计算出的视差矩阵
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud; // 计算出的点云

    void calculate_remapping_matrix();
};

#endif